//
// Created by ZhaoXiaoFei on 2022/8/5.
//

#ifndef POSE_GRAPH_KEY_FRAME_HPP
#define POSE_GRAPH_KEY_FRAME_HPP
#include <Eigen/Dense>
#include <opencv2/opencv.hpp>
#include <vector>
#include <list>
#include <sensor_msgs/PointCloud.h>
#include <glog/logging.h>
#include <opencv2/core/eigen.hpp>
#include "utility/parameters.hpp"
#include "ThirdParty/DVision/DVision.h"
#include "ThirdParty/DBoW/DBoW2.h"
#include "utility/utility.h"

template <class T>
static void reduceVector(std::vector<T>& data, std::vector<bool>& status){
    int j = 0;
    for(int i = 0; i < data.size(); i++){
        if(status[i]){
            data[j++] = data[i];
        }
    }
    data.resize(j);
}

class BriefExtractor{
public:
    BriefExtractor(const std::string& path){
        cv::FileStorage fs(path, cv::FileStorage::READ);
        if(!fs.isOpened()){
            throw(std::string("open fail"));
        }

        std::vector<int> x1, y1, x2, y2;
        fs["x1"] >> x1;
        fs["y1"] >> y1;
        fs["x2"] >> x2;
        fs["y2"] >> y2;

        m_brief.importPairs(x1, y1, x2, y2);

    }
    void operator()(const cv::Mat &image,
                    const std::vector<cv::KeyPoint> &points,
                    std::vector<DVision::BRIEF::bitset> &descriptors) const{
        m_brief.compute(image, points, descriptors);
    }
    DVision::BRIEF m_brief;
};

class KeyFrame{
public:
    KeyFrame(double time_stamp_, int index_, Eigen::Vector3d& vio_T_w_i_, Eigen::Matrix3d& vio_R_w_i_, cv::Mat& image_,
             std::vector<cv::Point3f>& point_3d_, std::vector<cv::Point2f>& point_2d_uv_, std::vector<cv::Point2f>& point_2d_xy_,
             std::vector<double>& point_id_, int sequence_);
    void computeWindowBRIEFPoint();
    void computeBriefPoint();
    bool searchInWindowByBF(const DVision::BRIEF::bitset window_descriptor,
                            const std::vector<DVision::BRIEF::bitset>& briefDescriptors_old,
                            const std::vector<cv::KeyPoint>& keyPoints_old,
                            const std::vector<cv::KeyPoint>& keyPoints_xy_old,
                            cv::Point2f &bestMatchPointUV,
                            cv::Point2f &bestMatchPointXY);
    void searchByBriefDistance(const std::vector<DVision::BRIEF::bitset>& briefDescriptors_old,
                               const std::vector<cv::KeyPoint>& keyPoints_old,
                               const std::vector<cv::KeyPoint>& keyPoints_xy_old,
                               std::vector<cv::Point2f>& matched_point2d_old,
                               std::vector<cv::Point2f>& matched_point2dXY_old,
                               std::vector<bool>& status);
    bool findConnection(KeyFrame* old_kf);
    void PnPByRansac(const std::vector<cv::Point2f>& matched_point2dXY_old,
                     const std::vector<cv::Point3f>& matched_point3d_cur,
                     Eigen::Matrix3d& R_old,
                     Eigen::Vector3d& T_old,
                     std::vector<bool>& status);

    inline int HammingDistance(const DVision::BRIEF::bitset& a, const DVision::BRIEF::bitset& b){
        return DVision::BRIEF::distance(a, b);
    }
    inline void getVioPose(Eigen::Matrix3d& vioR, Eigen::Vector3d& vioT){
        vioR = vio_R_w_i; vioT = vio_T_w_i;
    }
    inline void updateVioPose(const Eigen::Matrix3d& vioR, const Eigen::Vector3d& vioT){
        vio_R_w_i = vioR; vio_T_w_i = vioT;
        R_w_i = vioR; T_w_i = vioT;
    }
    inline void getPose(Eigen::Matrix3d& R, Eigen::Vector3d& T){
        R = R_w_i; T = T_w_i;
    }
    inline void updatePose(const Eigen::Matrix3d& R, const Eigen::Vector3d& T){
        R_w_i = R; T_w_i = T;
    }
    inline Eigen::Matrix3d getLoopRelativeR(){
        return Eigen::Quaterniond(loop_info(6), loop_info(3), loop_info(4), loop_info(5)).toRotationMatrix();
    }
    inline Eigen::Vector3d getLoopRelativeT(){
        return Eigen::Vector3d(loop_info(0), loop_info(1), loop_info(2));
    }
    inline double getLoopRelativeYaw(){
        return loop_info(7);
    }
    inline void updateLoop(Eigen::Matrix<double, 8, 1 > &_loop_info){
        if (abs(_loop_info(7)) < 30.0 && Eigen::Vector3d(_loop_info(0), _loop_info(1), _loop_info(2)).norm() < 20.0){
            loop_info = _loop_info;
        }
    }


    double time_stamp;
    int index;
    int local_index;

    Eigen::Matrix3d vio_R_w_i;
    Eigen::Vector3d vio_T_w_i;

    Eigen::Matrix3d R_w_i;
    Eigen::Vector3d T_w_i;

    Eigen::Matrix3d origin_vio_R;
    Eigen::Vector3d origin_vio_T;

    cv::Mat image;

    std::vector<cv::Point3f> point_3d;
    std::vector<cv::Point2f> point_2d_uv;
    std::vector<cv::Point2f> point_2d_xy;
    std::vector<double> point_id;

    std::vector<cv::KeyPoint> keyPoints;
    std::vector<cv::KeyPoint> keyPoints_xy;
    std::vector<DVision::BRIEF::bitset> briefDescriptors;

    std::vector<cv::KeyPoint> windowKeyPoints;
    std::vector<DVision::BRIEF::bitset> windowBriefDescriptors;
    int sequence;

    bool has_loop;
    int loop_index;
    Eigen::Matrix<double, 8, 1> loop_info;

};

#endif //POSE_GRAPH_KEY_FRAME_HPP
